#include "main.h"
#include "can.h"
#include "gpio.h"
#include "CAN_receive.h"
#include "detect_task.h"
#include "bsp_buzzer.h"
#include "remote_control.h"

toDownSTM_t DownSTM ;
#include "remote_control.h"

extern CAN_HandleTypeDef hcan1;
extern CAN_HandleTypeDef hcan2;

uint16_t psc = 100;
uint16_t pwm = 200;
  
//定义电机信息读取。实际是宏
#define get_motor_measure(ptr, data)                                    \
    {                                                                   \
        (ptr)->last_ecd = (ptr)->ecd;                                   \
        (ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]);            \
        (ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]);      \
        (ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]);  \
        (ptr)->temperate = (data)[6];                                   \
    }
/*
电机数据, 0:底盘电机1 3508电机,  1:底盘电机2 3508电机,2:底盘电机3 3508电机,3:底盘电机4 3508电机;
4:yaw云台电机 6020电机; 5:pitch云台电机 6020电机; 6:拨弹电机 2006电机*/
//6:拨弹电机 2006电机 发生更改

//用于存放所有信息。提供函数接口供外界使用
static motor_measure_t motor_gimbal[LENGTH_OF_MOTOR_ID];

//低层次的工作数据
static CAN_TxHeaderTypeDef  gimbal_tx_message;
static uint8_t              gimbal_can_send_data[8];

//回调函数。用于can的信息获取。在fifo结构内。
/**
  * @brief          hal库CAN回调函数,接收电机数据
  * @param[in]      hcan:CAN句柄指针
  * @retval         none
  */
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    CAN_RxHeaderTypeDef rx_header;
    uint8_t rx_data[8];

    HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
    if(hcan==&hcan1)
    {
      switch (rx_header.StdId)
      {
        case CAN_PITCH_UP:{
          get_motor_measure(&motor_gimbal[PITCH_UP],rx_data);
          detect_hook(PITCH_UP+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_UP_L:{
          get_motor_measure(&motor_gimbal[SHOOT_UP_L],rx_data);
          detect_hook(SHOOT_UP_L+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_UP_R:{
          get_motor_measure(&motor_gimbal[SHOOT_UP_R],rx_data);
          detect_hook(SHOOT_UP_R+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_UP_TRIGGER:{
          get_motor_measure(&motor_gimbal[SHOOT_UP_TRIGGER],rx_data);
          detect_hook(SHOOT_UP_TRIGGER+PITCH_UP_TOE);
          break;
        }
        case CAN_YAW_DOWN:{
          get_motor_measure(&motor_gimbal[YAW_DOWN],rx_data);
          detect_hook(YAW_DOWN+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_DOWN_F_TRIGGER:{
          get_motor_measure(&motor_gimbal[SHOOT_DOWN_F_TRIGGER],rx_data);
          detect_hook(SHOOT_DOWN_F_TRIGGER+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_DOWN_S_TRIGGER:{
          get_motor_measure(&motor_gimbal[SHOOT_DOWN_S_TRIGGER],rx_data);
          detect_hook(SHOOT_DOWN_S_TRIGGER+PITCH_UP_TOE);
          break;
        }
        default:{
            break;
        }
      }
    } else if(hcan==&hcan2)
    {
      switch (rx_header.StdId)
      {
        case CAN_YAW_UP:{
          get_motor_measure(&motor_gimbal[YAW_UP],rx_data);
          detect_hook(YAW_UP+PITCH_UP_TOE);
          break;
        }
        case CAN_PITCH_DOWN:{
          get_motor_measure(&motor_gimbal[PITCH_DOWN],rx_data);
          detect_hook(PITCH_DOWN+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_DOWN_L:{
          get_motor_measure(&motor_gimbal[SHOOT_DOWN_L],rx_data);
          detect_hook(SHOOT_DOWN_L+PITCH_UP_TOE);
          break;
        }
        case CAN_SHOOT_DOWN_R:{
          get_motor_measure(&motor_gimbal[SHOOT_DOWN_R],rx_data);
          detect_hook(SHOOT_DOWN_R+PITCH_UP_TOE);
          break;
        }
        default:{
          break;
        }
      }
    }
}


/**
  * @brief[in]      向底盘发送下云台拨弹信息和yawdownecd
  * @param[in]      motor_gimbal[YAW_DOWN].ecd：yawdownecd
  * @param[in]      shootdownFtrigger: 2006电机控制电流,  [-10000，10000]MAX=10A
  * @param[in]      shootdownStrigger: 2006电机控制电流,  [-10000，10000]MAX=10A
  * @retval         none
  */

void CAN_cmd_downtrigger( int16_t shootdownFtrigger, int16_t shootdownStrigger)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_DOWN_OTHER_CAN1_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    
    gimbal_can_send_data[0] = 0;
    gimbal_can_send_data[1] = 0;
    gimbal_can_send_data[2] = 0;
    gimbal_can_send_data[3] = 0;
    gimbal_can_send_data[4] = shootdownFtrigger >> 8;
    gimbal_can_send_data[5] = shootdownFtrigger;
    gimbal_can_send_data[6] = shootdownStrigger >> 8;
    gimbal_can_send_data[7] = shootdownStrigger;
    
    HAL_CAN_AddTxMessage(&TOCHASSIS_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          发送下云台电机控制电压(0x209,0x20A)
  * @param[in]      yawup: (0x209) 6020电机控制电压, 范围 [-30000,30000]
  * @param[in]      pitchdown: (0x20A) 6020电机控制电压, 范围 [-30000,30000]
  * @retval         none
  */
void CAN_cmd_downgimbal(int16_t yawup, int16_t pitchdown)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_DOWN_G_CAN2_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    
    gimbal_can_send_data[0] = yawup >> 8;
    gimbal_can_send_data[1] = yawup;
    gimbal_can_send_data[2] = pitchdown >> 8;
    gimbal_can_send_data[3] = pitchdown;
    gimbal_can_send_data[4] = 0;
    gimbal_can_send_data[5] = 0;
    gimbal_can_send_data[6] = 0;
    gimbal_can_send_data[7] = 0;
    
    HAL_CAN_AddTxMessage(&DOWN_GIMBAL_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          发送下云台电机控制电流(0x205,0x206)
  * @param[in]      shootdwonL: (0x205) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      shootdownR: (0x206) 3508电机控制电流, 范围 [-16384,16384]
  * @retval         none
  */
void CAN_cmd_downshoot(int16_t shootdownL, int16_t shootdownR)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_DOWN_M_CAN2_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    
    gimbal_can_send_data[0] = shootdownL >> 8;
    gimbal_can_send_data[1] = shootdownL;
    gimbal_can_send_data[2] = shootdownR >> 8;
    gimbal_can_send_data[3] = shootdownR;
    gimbal_can_send_data[4] = 0;
    gimbal_can_send_data[5] = 0;
    gimbal_can_send_data[6] = 0;
    gimbal_can_send_data[7] = 0;

    HAL_CAN_AddTxMessage(&DOWN_SHOOT_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          发送上云台电机控制电流(0x202,0x203)
  * @param[in]      shootupL: (0x202) 3508电机控制电流, 范围 [-16384,16384]
  * @param[in]      shootupR: (0x203) 3508电机控制电流, 范围[-16384,16384]
  * @param[in]      shootuptrigger: (0x204) 2006电机控制电流, 范围 [-10000，10000]MAX=10A
  * @retval         none
  */
void CAN_cmd_upshoot(int16_t shootupL, int16_t shootupR, int16_t shootuptriggger)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_UP_M_CAN1_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    
    gimbal_can_send_data[0] = 0;
    gimbal_can_send_data[1] = 0;
    gimbal_can_send_data[2] = shootupL >> 8;
    gimbal_can_send_data[3] = shootupL;
    gimbal_can_send_data[4] = shootupR >> 8;
    gimbal_can_send_data[5] = shootupR;
    gimbal_can_send_data[6] = shootuptriggger >> 8;
    gimbal_can_send_data[7] = shootuptriggger;
    
    HAL_CAN_AddTxMessage(&UP_SHOOT_CAN , &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}

/**
  * @brief          向底盘发送信息
  * @param[in]      yawDown: (0x)
  * @param[in]      pitchup: (0x209) 6020电机控制电压, 范围 [-30000,30000]
  * @retval         none
  */

/**
  * @brief          发送上云台电机控制电流(0x204)
  * @param[in]      shootuptrigger: (0x204) 2006电机控制电流, 范围 [-10000，10000]MAX=10A
  * @retval         none
  */

/**
  * @brief          发送上云台pitch和下云台yaw以及给底盘C板的值
  * @brief          发送到底盘(1,2，3，4)
  * @param[in]      shootuptrigger: (0x204) 2006电机控制电流, 范围 [-10000，10000]MAX=10A
  * @retval         none
  */
void CAN_cmd_GM_and_downSTM(int16_t yawDown,int16_t pitchUp)
{
    uint32_t send_mail_box;
    gimbal_tx_message.StdId = CAN_DOWN_G_CAN1_ID;
    gimbal_tx_message.IDE = CAN_ID_STD;
    gimbal_tx_message.RTR = CAN_RTR_DATA;
    gimbal_tx_message.DLC = 0x08;
    
    gimbal_can_send_data[0] = yawDown >> 8;
    gimbal_can_send_data[1] = yawDown;
    gimbal_can_send_data[2] = pitchUp>>8;
    gimbal_can_send_data[3] = pitchUp;
    gimbal_can_send_data[4] = DownSTM.x;
    gimbal_can_send_data[5] = DownSTM.y;
    gimbal_can_send_data[6] = DownSTM.w;
    gimbal_can_send_data[7] = DownSTM.nextmode;
    
    HAL_CAN_AddTxMessage(&UP_SHOOT_CAN, &gimbal_tx_message, gimbal_can_send_data, &send_mail_box);
}





//一些用于传递参数的函数，供给pid控制

/**
  * @brief          返回云台电机 3508电机数据指针
  * @param[in]      i: 电机编号,范围[0,10]
  * @retval         电机数据指针
  */
const motor_measure_t *get_gimbal_motor_measure_point(uint8_t i)
{
    return &motor_gimbal[i];
}

/**
  * @brief          返回向底盘发送的的数据结构体指针
  * @retval         向底盘发送的数据结构体指针
  */
toDownSTM_t * get_to_down_stm_point(void)
{
    return &DownSTM;
}
